using System;
using System.Collections.Generic;

namespace MRIData
{
	/// <summary>
	/// Implements the quickhull algorithm for a set of 3-dimensional points
	/// </summary>
	/// This static class implements the quickhull algorithm to find the convex hull of a set of 3-dimensional 
	/// <exception cref='Exception'>
	/// Represents errors that occur during application execution.
	/// </exception>
	internal static class QuickHull
	{
		static readonly double ZERO_TESTING_THRESHOLD = 1e-16; 
		
		public static Mesh ConvexHull(double[,] vertices)
		{
			if(vertices.GetLength(1) != 3)
			{
				throw new Exception("Parameter 'vertices' invalid. Dimension 1 must have size 3.");
			}
			
			int numVerticesIn 		= vertices.GetLength(0);
			int numVerticesMesh		= 4;
			int numVerticesRest		= numVerticesIn - 4;
			
			List<double[]> full_vertices 	= new List<double[]>();
			List<double[]> hull_vertices 	= new List<double[]>();
			List<int[]> hull_triangles 		= new List<int[]>();
			
			//start by selecting first point
			double[] p1 = SelectRow(vertices, 0);
			double[] p2 = {0.0, 0.0, 0.0};
			double[] p3 = {0.0, 0.0, 0.0};
			double[] p4 = {0.0, 0.0, 0.0};
			
			double maxDist = 0.0;
			int maxDistIndex = 0;
			
			//copy vertices to list
			for(int i = 1; i<numVerticesIn; i++)
			{
				full_vertices.Add(SelectRow(vertices,i));
			}
			
			//select furthest point from first
			for(int i=0; i<numVerticesIn-1; i++)
			{
				double dist = Math.Sqrt((p1[0] - full_vertices[i][0])*(p1[0]-vertices[i][0]) + (p1[1]-vertices[i][1])*(p1[1]-vertices[i][1])+(p1[2]-vertices[i][2])*(p1[2]-vertices[i][2]));
				
				if(dist > maxDist)
				{
					maxDist = dist;
					maxDistIndex = i;
				}
			}
			
			if(Math.Abs(maxDist) <= ZERO_TESTING_THRESHOLD)
			{
				throw new Exception("Dataset is degenerate (0-d)");
			}
			
			p2 = full_vertices[maxDistIndex];
			full_vertices.RemoveAt(maxDistIndex);
			
			//select point furthest from line connecting first two points
			maxDist = 0.0;
			maxDistIndex = 0;
			
			for(int i=0; i<numVerticesIn-2; i++)
			{
				double dist = LinePointDist(p1, p2, full_vertices[i]);
				
				if(dist > maxDist)
				{
					maxDist = dist;
					maxDistIndex = i;
				}
			}
			
			if(Math.Abs(maxDist) <= ZERO_TESTING_THRESHOLD)
			{
				throw new Exception("Dataset is degenerate (1-d)");
			}
			
			p3 = full_vertices[maxDistIndex];
			full_vertices.RemoveAt(maxDistIndex);
			
			
			//select furthest point from triangle
			maxDist = 0.0;
			maxDistIndex = 0;
			
			for(int i=0; i<numVerticesIn-3; i++)
			{
				double dist = PlanePointDist(p1, p2, p3, full_vertices[i]);
				
				if(dist > maxDist)
				{
					maxDist = dist;
					maxDistIndex = i;
				}
			}
			
			if(Math.Abs(maxDist) <= ZERO_TESTING_THRESHOLD)
			{
				throw new Exception("Dataset is degenerate (2-d)");
			}
			
			p4 = full_vertices[maxDistIndex];
			full_vertices.RemoveAt(maxDistIndex);
			
			hull_vertices.Add(p1);
			hull_vertices.Add(p2);
			hull_vertices.Add(p3);
			hull_vertices.Add(p4);
			
			hull_triangles.Add(new int[] {0, 1, 2});
			hull_triangles.Add(new int[] {0, 3, 1});
			hull_triangles.Add(new int[] {0, 2, 3});
			hull_triangles.Add(new int[] {1, 3, 2});
			
			
			
			//
			return QuickHullRecursive(vertices, p1, p2, p3);
			
			
		}
		
		private static Mesh QuickHullRecursive(double[,] vertices, params double[][] tri)
		{
			//find furthest point from triangle given by 'tri'
			return null;
		}
		
		private static double[] SelectRow(double[,] matrix, int rowNumber)
		{
			int rowLength = matrix.GetLength(1);
			double[] row = new double[rowLength];
			
			for(int i=0; i<rowLength; i++)
			{
				row[i] = matrix[rowNumber, i];
			}
			
			return row;
		}
		
		private static double LinePointDist(double[] p1, double[] p2, double[] ptest)
		{
			if(p1.Length != 3 || p2.Length != 3 || ptest.Length != 3)
			{
				throw new Exception("Any of the parameters is not 3-dimensional. Only 3-dimensional sapces are supported by now.");
			}
			
			double[] d = {0.0, 0.0, 0.0};
			double[] n = {0.0, 0.0, 0.0};
			double s = 0.0;
			
			d[0] = p2[0]-p1[0];
			d[1] = p2[1]-p1[1];
			d[2] = p2[2]-p1[2];
			
			s = d[0]*d[0]+d[1]*d[1]+d[2]*d[2];
			
			n[0] = d[0]*d[1]*(p1[1]-ptest[1])/s - (p1[0]-ptest[0])*(d[1]*d[1]+d[2]*d[2])/s + d[0]*d[2]*(p1[2]-ptest[2])/s;
			n[1] = d[0]*d[1]*(p1[0]-ptest[0])/s - (p1[1]-ptest[1])*(d[0]*d[0]+d[2]*d[2])/s + d[1]*d[2]*(p1[2]-ptest[2])/s;
			n[2] = d[0]*d[2]*(p1[0]-ptest[0])/s - (p1[2]-ptest[2])*(d[0]*d[0]+d[1]*d[1])/s + d[1]*d[2]*(p1[1]-ptest[1])/s;
			
			//a is ommited
			
			return Math.Sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
		}
		
		private static double PlanePointDist(double[] p1, double[] p2, double[] p3, double[] ptest)
		{
			double[] d1 = {0.0, 0.0, 0.0};
			double[] d2 = {0.0, 0.0, 0.0};
			double[] c = {0.0, 0.0, 0.0};
			
			double s = 0.0;
			double gamma = 0.0;
			
			d1[0] = p2[0]-p1[0];
			d1[1] = p2[1]-p1[1];
			d1[2] = p2[2]-p1[2];
			
			d2[0] = p3[0]-p1[0];
			d2[1] = p3[1]-p1[1];
			d2[2] = p3[2]-p1[2];
			
			c[0] = d1[2]*d2[1]-d1[1]*d2[2];
			c[1] = d1[0]*d2[2]-d1[2]*d2[0];
			c[3] = d1[1]*d2[0]-d1[0]*d2[1];
			
			s = c[0]*d1[1]*d2[2]-c[0]*d1[2]*d2[1]-c[1]*d1[0]*d2[2]+c[1]*d1[2]*d2[0]+c[2]*d1[0]*d2[1]-c[2]*d1[1]*d2[0];
			
			//omit alpha and beta
			gamma = (p1[1]-ptest[1])*(d1[0]*d2[2]-d1[2]*d2[0])/s - (p1[2]-ptest[2])*(d1[0]*d2[1]-d1[1]*d2[0])/s - (p1[0]-ptest[0])*(d1[1]*d2[2]-d1[2]*d2[1])/s;
			
			return gamma * Math.Sqrt (c[0]*c[0]+c[1]*c[1]+c[2]*c[2]);
		}
		
		static double[][] GetTriangle(List<double[]> vertices, int[] triangle)
		{
			double[][] tri;
			
			
			for(int i=0; i<triangle.Length; i++)
			{
				tri[i] = vertices[i];
			}
			
			return tri;
		}
	}
}

